#include <iostream>
#include <cstring>
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <thread>
#include <atomic>
#include <random>
#include <chrono>
#include <mutex>  // 添加互斥锁头文件
#include <stdexcept>  // 添加异常处理头文件

class ThrusterDriver {
public:
    ThrusterDriver(uint8_t thrusterNum, const std::string& ifname = "vcan0") 
        : thrusterNum_(thrusterNum), running_(false) {

        // 创建CAN socket
        if ((sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
            throw std::runtime_error("Socket creation failed");
        }

        // 绑定CAN接口
        strcpy(ifr_.ifr_name, ifname.c_str());
        if (ioctl(sock_, SIOCGIFINDEX, &ifr_) < 0) {
            close(sock_);
            throw std::runtime_error("CAN interface ioctl failed");
        }

        addr_.can_family = AF_CAN;
        addr_.can_ifindex = ifr_.ifr_ifindex;
        if (bind(sock_, (struct sockaddr *)&addr_, sizeof(addr_)) < 0) {
            close(sock_);
            throw std::runtime_error("Bind failed");
        }

        // 启用扩展帧支持
        int enable = 1;
        if (setsockopt(sock_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, sizeof(enable)) < 0) {
            close(sock_);
            throw std::runtime_error("Failed to enable extended frames");
        }

        // 初始化CAN ID
        // 初始化CAN ID (使用无符号类型)
        feedback_id_ = 0x321121U;
        control_id_ = 0x32111U;
        //auto_feedback1_id_ = 0x321100U | (static_cast<uint32_t>(thrusterNum_)) | 0x21U | CAN_EFF_FLAG;
        auto_feedback1_id_ = 0x321100U | 0x22U | CAN_EFF_FLAG;
        auto_feedback2_id_ = 0x321100U | 0x23U | CAN_EFF_FLAG;
        auto_feedback3_id_ = 0x321100U | 0x24U | CAN_EFF_FLAG;
        auto_feedback4_id_ = 0x321100U | 0x25U | CAN_EFF_FLAG;
        auto_feedback5_id_ = 0x321100U | 0x26U | CAN_EFF_FLAG;
        auto_feedback6_id_ = 0x321100U | 0x27U | CAN_EFF_FLAG;
        auto_feedback7_id_ = 0x321100U | 0x28U | CAN_EFF_FLAG;
        auto_feedback8_id_ = 0x321100U | 0x29U | CAN_EFF_FLAG;
        auto_feedback9_id_ = 0x321100U | 0x30U | CAN_EFF_FLAG;
        auto_feedback10_id_ = 0x321100U | 0x31U | CAN_EFF_FLAG;
        auto_feedback11_id_ = 0x321100U | 0x32U | CAN_EFF_FLAG;
        auto_feedback12_id_ = 0x321100U | 0x33U | CAN_EFF_FLAG;

    }

    ~ThrusterDriver() {
        stop();
        close(sock_);
    }

    void start() {
        if (running_) return;
        
        running_ = true;
        
        // 启动接收线程
        recv_thread_ = std::thread(&ThrusterDriver::receiveLoop, this);
        
        // 启动自动反馈线程
        feedback_thread1_ = std::thread(&ThrusterDriver::autoFeedback1Loop, this);
        feedback_thread2_ = std::thread(&ThrusterDriver::autoFeedback2Loop, this);
        feedback_thread3_ = std::thread(&ThrusterDriver::autoFeedback3Loop, this);
        feedback_thread4_ = std::thread(&ThrusterDriver::autoFeedback4Loop, this);
        feedback_thread5_ = std::thread(&ThrusterDriver::autoFeedback5Loop, this);
        feedback_thread6_ = std::thread(&ThrusterDriver::autoFeedback6Loop, this);
        feedback_thread7_ = std::thread(&ThrusterDriver::autoFeedback7Loop, this);
        feedback_thread8_ = std::thread(&ThrusterDriver::autoFeedback8Loop, this);
        feedback_thread9_ = std::thread(&ThrusterDriver::autoFeedback9Loop, this);
        feedback_thread10_ = std::thread(&ThrusterDriver::autoFeedback10Loop, this);
        feedback_thread11_ = std::thread(&ThrusterDriver::autoFeedback11Loop, this);
        feedback_thread12_ = std::thread(&ThrusterDriver::autoFeedback12Loop, this);
    }

    void stop() {
        running_ = false;
        
        if (recv_thread_.joinable()) recv_thread_.join();
        if (feedback_thread1_.joinable()) feedback_thread1_.join();
        if (feedback_thread2_.joinable()) feedback_thread2_.join();
        if (feedback_thread3_.joinable()) feedback_thread3_.join();
        if (feedback_thread4_.joinable()) feedback_thread4_.join();
        if (feedback_thread5_.joinable()) feedback_thread5_.join();
        if (feedback_thread6_.joinable()) feedback_thread6_.join();
        if (feedback_thread7_.joinable()) feedback_thread7_.join();
        if (feedback_thread8_.joinable()) feedback_thread8_.join();
        if (feedback_thread9_.joinable()) feedback_thread9_.join();
        if (feedback_thread10_.joinable()) feedback_thread10_.join();
        if (feedback_thread11_.joinable()) feedback_thread11_.join();
        if (feedback_thread12_.joinable()) feedback_thread12_.join();
    }

private:

    uint32_t feedback_id_;
    uint32_t auto_feedback1_id_;
    uint32_t auto_feedback2_id_;
    uint32_t auto_feedback3_id_;
    uint32_t auto_feedback4_id_;
    uint32_t auto_feedback5_id_;
    uint32_t auto_feedback6_id_;
    uint32_t auto_feedback7_id_;
    uint32_t auto_feedback8_id_;
    uint32_t auto_feedback9_id_;
    uint32_t auto_feedback10_id_;
    uint32_t auto_feedback11_id_;
    uint32_t auto_feedback12_id_;
    uint32_t control_id_;  // 添加控制帧ID
        std::default_random_engine rng;

    void receiveLoop() {
        struct can_frame frame;
        
        while (running_) {
            if (read(sock_, &frame, sizeof(frame)) < 0) {
                if (running_) perror("Read error");
                continue;
            }
            // 输出所有收到的CAN ID
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Received CAN ID: 0x" << std::hex << frameID;
            }

            // 检查是否是本推进器的控制帧 (ID: 0x32111)
            if ((frameID & 0x1FFFFFFF) == control_id_) {
                // 解析转速和故障清零标志
                int16_t speed = frame.data[4] | (frame.data[5] << 8);
                bool fault_clear = frame.data[2] & 0x01;

                {
                    std::lock_guard<std::mutex> lock(data_mutex_);
                    current_speed_ = speed;
                    if (fault_clear) fault_flags1_ = fault_flags2_ = 0;
                }

                // 立即发送反馈帧
                sendFeedbackFrame(speed, fault_clear);
                {
                    std::lock_guard<std::mutex> lock(print_mutex_);
                    std::cout << "\t我已收到" <<std::dec << current_speed_ << "rpm" << std::endl;
                }
            }else{
                {
                    std::lock_guard<std::mutex> lock(print_mutex_);
                    std::cout << "\t我已收到" << std::endl;
                }
            }
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << std::endl;std::cout << std::endl;
            }
        }
    }

    void sendFeedbackFrame(int16_t speed, bool fault_clear) {
        struct can_frame frame;
        memset(&frame, 0, sizeof(frame));

        frame.can_id = feedback_id_;
        frame.can_dlc = 8;
        
        uint32_t frameID = frame.can_id & CAN_EFF_MASK;
        {
            std::lock_guard<std::mutex> lock(print_mutex_);
            std::cout << "Sent CAN ID: 0x" << std::hex << frameID << std::endl;
        }

        // 第0字节为随机数
        std::uniform_int_distribution<uint8_t> rand_dist(0, 255);
        frame.data[0] = rand_dist(rng);
        // 第4、5字节为转速
        frame.data[4] = speed & 0xFF;         // 低8位
        frame.data[5] = (speed >> 8) & 0xFF;  // 高8位
        // 其他字节保留为0

        if (write(sock_, &frame, sizeof(frame)) < 0) {
            perror("Feedback frame send failed");
        }
    }

    void autoFeedback1Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<int16_t> speed_dist(-1000, 1000);  // -100.0~100.0, 0.1rpm精度
        std::uniform_int_distribution<uint16_t> input_volt_dist(0, 600); // 0~600V, 1V精度
        std::uniform_int_distribution<uint16_t> bus_volt_dist(0, 800);   // 0~800V, 1V精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback1_id_;
            frame.can_dlc = 8;
            //std::cout << "ID: " << std::hex << frame.can_id << std::endl;
            // data[0][1] 保留
            frame.data[0] = 0;
            frame.data[1] = 0;

            // 转速（有符号16位，0.1rpm，-100~100）
            int16_t speed = speed_dist(gen);
            frame.data[2] = speed & 0xFF;         // 低8位
            frame.data[3] = (speed >> 8) & 0xFF;  // 高8位

            // 输入电压（无符号16位，1V，0~600）
            uint16_t input_volt = input_volt_dist(gen);
            frame.data[4] = input_volt & 0xFF;         // 低8位
            frame.data[5] = (input_volt >> 8) & 0xFF;  // 高8位

            // 母线电压（无符号16位，1V，0~800）
            uint16_t bus_volt = bus_volt_dist(gen);
            frame.data[6] = bus_volt & 0xFF;         // 低8位
            frame.data[7] = (bus_volt >> 8) & 0xFF;  // 高8位

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback1 send failed");
            }

            // 输出发送的CAN ID和转速、电压信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t转速: " << std::dec << speed << " (0.1rpm)"
                         << "\t输入电压: " << input_volt << "V"
                         << "\t母线电压: " << bus_volt << "V" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback2Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint16_t> input_current_dist(0, 600);  // 支路一输入电流 0-600A, 1A精度
        std::uniform_int_distribution<uint16_t> motor_current_dist(0, 500);  // 电机电流 0-500A, 1A精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback2_id_;
            frame.can_dlc = 8;
            
            // 支路一输入电流（无符号16位，1A精度，0-600A）
            uint16_t input_current = input_current_dist(gen);
            frame.data[0] = input_current & 0xFF;         // 低8位
            frame.data[1] = (input_current >> 8) & 0xFF;  // 高8位

            // 支路一电机电流1A（无符号16位，1A精度，0-500A）
            uint16_t motor_current_1a = motor_current_dist(gen);
            frame.data[2] = motor_current_1a & 0xFF;         // 低8位
            frame.data[3] = (motor_current_1a >> 8) & 0xFF;  // 高8位

            // 支路一电机电流1B（无符号16位，1A精度，0-500A）
            uint16_t motor_current_1b = motor_current_dist(gen);
            frame.data[4] = motor_current_1b & 0xFF;         // 低8位
            frame.data[5] = (motor_current_1b >> 8) & 0xFF;  // 高8位

            // 支路一电机电流1C（无符号16位，1A精度，0-500A）
            uint16_t motor_current_1c = motor_current_dist(gen);
            frame.data[6] = motor_current_1c & 0xFF;         // 低8位
            frame.data[7] = (motor_current_1c >> 8) & 0xFF;  // 高8位

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback2 send failed");
            }

            // 输出发送的CAN ID和电流信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t支路一输入电流: " << std::dec << input_current << "A"
                         << "\t支路一电机电流1A: " << motor_current_1a << "A"
                         << "\t支路一电机电流1B: " << motor_current_1b << "A"
                         << "\t支路一电机电流1C: " << motor_current_1c << "A" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback3Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> status_dist(0, 255);  // 状态随机数 0-255
        std::uniform_int_distribution<int16_t> speed_dist(-1000, 1000);  // 支路二反馈转速 -100~100, 0.1rpm精度
        std::uniform_int_distribution<uint16_t> input_volt_dist(0, 600); // 支路二输入电压 0~600V, 1V精度
        std::uniform_int_distribution<uint16_t> bus_volt_dist(0, 800);   // 支路二母线电压 0~800V, 1V精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback3_id_;
            frame.can_dlc = 8;
            
            // 状态随机数（0-255，后面要改成状态定义）
            frame.data[0] = status_dist(gen);
            
            // 保留为0
            frame.data[1] = 0;
            
            // 支路二反馈转速（有符号16位，0.1rpm精度，-100~100）
            int16_t speed = speed_dist(gen);
            frame.data[2] = speed & 0xFF;         // 低8位
            frame.data[3] = (speed >> 8) & 0xFF;  // 高8位

            // 支路二输入电压（无符号16位，1V精度，0~600）
            uint16_t input_volt = input_volt_dist(gen);
            frame.data[4] = input_volt & 0xFF;         // 低8位
            frame.data[5] = (input_volt >> 8) & 0xFF;  // 高8位

            // 支路二母线电压（无符号16位，1V精度，0~800）
            uint16_t bus_volt = bus_volt_dist(gen);
            frame.data[6] = bus_volt & 0xFF;         // 低8位
            frame.data[7] = (bus_volt >> 8) & 0xFF;  // 高8位

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback3 send failed");
            }

            // 输出发送的CAN ID和支路二信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t支路二状态: " << std::dec << static_cast<int>(frame.data[0])
                         << "\t支路二反馈转速: " << speed << " (0.1rpm)"
                         << "\t支路二输入电压: " << input_volt << "V"
                         << "\t支路二母线电压: " << bus_volt << "V" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback4Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint16_t> current_dist(0, 500);  // 电流 0-500A, 1A精度
        std::uniform_int_distribution<uint16_t> temp_dist(0, 100);     // 温度 0-100℃, 1℃精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback4_id_;
            frame.can_dlc = 8;
            
            // 支路二电机电流2A（无符号16位，1A精度，0-500A）
            uint16_t motor_current_2a = current_dist(gen);
            frame.data[0] = motor_current_2a & 0xFF;         // 低8位
            frame.data[1] = (motor_current_2a >> 8) & 0xFF;  // 高8位

            // 支路二电机电流2B（无符号16位，1A精度，0-500A）
            uint16_t motor_current_2b = current_dist(gen);
            frame.data[2] = motor_current_2b & 0xFF;         // 低8位
            frame.data[3] = (motor_current_2b >> 8) & 0xFF;  // 高8位

            // 支路二电机电流2C（无符号16位，1A精度，0-500A）
            uint16_t motor_current_2c = current_dist(gen);
            frame.data[4] = motor_current_2c & 0xFF;         // 低8位
            frame.data[5] = (motor_current_2c >> 8) & 0xFF;  // 高8位

            // 支路二输入电流（无符号16位，1A精度，0-600A）
            uint16_t input_current = current_dist(gen);
            frame.data[6] = input_current & 0xFF;         // 低8位
            frame.data[7] = (input_current >> 8) & 0xFF;  // 高8位

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback4 send failed");
            }

            // 输出发送的CAN ID和电流信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t支路二电机电流2A: " << std::dec << motor_current_2a << "A"
                         << "\t支路二电机电流2B: " << motor_current_2b << "A"
                         << "\t支路二电机电流2C: " << motor_current_2c << "A"
                         << "\t支路二输入电流: " << input_current << "A" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback5Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> temp_dist(0, 150);  // 轴承温度 0-150℃, 1℃精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback5_id_;
            frame.can_dlc = 8;
            
            // 0-3保留为0
            frame.data[0] = 0;
            frame.data[1] = 0;
            frame.data[2] = 0;
            frame.data[3] = 0;

            // 电机驱动端轴承温度1（无符号8位，1℃精度，0-150℃）
            frame.data[4] = temp_dist(gen);

            // 电机驱动端轴承温度2（无符号8位，1℃精度，0-150℃）
            frame.data[5] = temp_dist(gen);

            // 电机驱动端轴承温度3（无符号8位，1℃精度，0-150℃）
            frame.data[6] = temp_dist(gen);

            // 电机驱动端轴承温度4（无符号8位，1℃精度，0-150℃）
            frame.data[7] = temp_dist(gen);

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback5 send failed");
            }

            // 输出发送的CAN ID和轴承温度信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t电机驱动端轴承温度1: " << std::dec << static_cast<int>(frame.data[4]) << "℃"
                         << "\t电机驱动端轴承温度2: " << static_cast<int>(frame.data[5]) << "℃"
                         << "\t电机驱动端轴承温度3: " << static_cast<int>(frame.data[6]) << "℃"
                         << "\t电机驱动端轴承温度4: " << static_cast<int>(frame.data[7]) << "℃" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback6Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> temp_dist(0, 150);  // 温度 0-150℃, 1℃精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback6_id_;
            frame.can_dlc = 8;
            
            // 电机驱动端轴承温度5（无符号8位，1℃精度，0-150℃）
            frame.data[0] = temp_dist(gen);

            // 电机驱动端轴承温度6（无符号8位，1℃精度，0-150℃）
            frame.data[1] = temp_dist(gen);

            // 电机非驱动端轴承温度1（无符号8位，1℃精度，0-150℃）
            frame.data[2] = temp_dist(gen);

            // 电机非驱动端轴承温度2（无符号8位，1℃精度，0-150℃）
            frame.data[3] = temp_dist(gen);

            // 电机绕组端部温度1（无符号8位，1℃精度，0-150℃）
            frame.data[4] = temp_dist(gen);

            // 电机绕组端部温度2（无符号8位，1℃精度，0-150℃）
            frame.data[5] = temp_dist(gen);

            // 电机绕组端部温度3（无符号8位，1℃精度，0-150℃）
            frame.data[6] = temp_dist(gen);

            // 电机绕组端部温度4（无符号8位，1℃精度，0-150℃）
            frame.data[7] = temp_dist(gen);

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback6 send failed");
            }

            // 输出发送的CAN ID和温度信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t电机驱动端轴承温度5: " << std::dec << static_cast<int>(frame.data[0]) << "℃"
                         << "\t电机驱动端轴承温度6: " << static_cast<int>(frame.data[1]) << "℃"
                         << "\t电机非驱动端轴承温度1: " << static_cast<int>(frame.data[2]) << "℃"
                         << "\t电机非驱动端轴承温度2: " << static_cast<int>(frame.data[3]) << "℃"
                         << "\t电机绕组端部温度1: " << static_cast<int>(frame.data[4]) << "℃"
                         << "\t电机绕组端部温度2: " << static_cast<int>(frame.data[5]) << "℃"
                         << "\t电机绕组端部温度3: " << static_cast<int>(frame.data[6]) << "℃"
                         << "\t电机绕组端部温度4: " << static_cast<int>(frame.data[7]) << "℃" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback7Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> temp_dist(0, 150);  // 温度 0-150℃, 1℃精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback7_id_;
            frame.can_dlc = 8;
            
            // 电机绕组端部温度5（无符号8位，1℃精度，0-150℃）
            frame.data[0] = temp_dist(gen);

            // 电机绕组端部温度6（无符号8位，1℃精度，0-150℃）
            frame.data[1] = temp_dist(gen);

            // 电机油液温度（无符号8位，1℃精度，0-150℃）
            frame.data[2] = temp_dist(gen);

            // 变频器输入滤波器温度1（无符号8位，1℃精度，0-150℃）
            frame.data[3] = temp_dist(gen);

            // 变频器输入滤波器温度2（无符号8位，1℃精度，0-150℃）
            frame.data[4] = temp_dist(gen);

            // 变频器输入滤波器温度3（无符号8位，1℃精度，0-150℃）
            frame.data[5] = temp_dist(gen);

            // 变频器输入滤波器温度4（无符号8位，1℃精度，0-150℃）
            frame.data[6] = temp_dist(gen);

            // 变频器散热板温度1（无符号8位，1℃精度，0-150℃）
            frame.data[7] = temp_dist(gen);

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback7 send failed");
            }

            // 输出发送的CAN ID和温度信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t电机绕组端部温度5: " << std::dec << static_cast<int>(frame.data[0]) << "℃"
                         << "\t电机绕组端部温度6: " << static_cast<int>(frame.data[1]) << "℃"
                         << "\t电机油液温度: " << static_cast<int>(frame.data[2]) << "℃"
                         << "\t变频器输入滤波器温度1: " << static_cast<int>(frame.data[3]) << "℃"
                         << "\t变频器输入滤波器温度2: " << static_cast<int>(frame.data[4]) << "℃"
                         << "\t变频器输入滤波器温度3: " << static_cast<int>(frame.data[5]) << "℃"
                         << "\t变频器输入滤波器温度4: " << static_cast<int>(frame.data[6]) << "℃"
                         << "\t变频器散热板温度1: " << static_cast<int>(frame.data[7]) << "℃" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback8Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> temp_dist(0, 150);  // 温度 0-150℃, 1℃精度
        std::uniform_int_distribution<uint8_t> level_dist(0, 100); // 液位 0-100%, 1%精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback8_id_;
            frame.can_dlc = 8;
            
            // 变频器散热板温度2（无符号8位，1℃精度，0-150℃）
            frame.data[0] = temp_dist(gen);

            // 变频器预充电散热板温度3（无符号8位，1℃精度，0-150℃）
            frame.data[1] = temp_dist(gen);

            // 变频器制动散热板温度4（无符号8位，1℃精度，0-150℃）
            frame.data[2] = temp_dist(gen);

            // 进口水温度（无符号8位，1℃精度，0-150℃）
            frame.data[3] = temp_dist(gen);

            // 补偿器液位1（无符号8位，1%精度，0-100%）
            frame.data[4] = level_dist(gen);

            // 补偿器液位2（无符号8位，1%精度，0-100%）
            frame.data[5] = level_dist(gen);

            // 补偿器液位3（无符号8位，1%精度，0-100%）
            frame.data[6] = level_dist(gen);

            // 补偿器液位4（无符号8位，1%精度，0-100%）
            frame.data[7] = level_dist(gen);

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback8 send failed");
            }

            // 输出发送的CAN ID和温度、液位信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t变频器散热板温度2: " << std::dec << static_cast<int>(frame.data[0]) << "℃"
                         << "\t变频器预充电散热板温度3: " << static_cast<int>(frame.data[1]) << "℃"
                         << "\t变频器制动散热板温度4: " << static_cast<int>(frame.data[2]) << "℃"
                         << "\t进口水温度: " << static_cast<int>(frame.data[3]) << "℃"
                         << "\t补偿器液位1: " << static_cast<int>(frame.data[4]) << "%"
                         << "\t补偿器液位2: " << static_cast<int>(frame.data[5]) << "%"
                         << "\t补偿器液位3: " << static_cast<int>(frame.data[6]) << "%"
                         << "\t补偿器液位4: " << static_cast<int>(frame.data[7]) << "%" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback9Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint16_t> pressure_dist(0, 1000); // 压力 0-1000kPa, 1kPa精度
        std::uniform_int_distribution<uint8_t> temp_dist(0, 150);       // 温度 0-150℃, 1℃精度

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback9_id_;
            frame.can_dlc = 8;
            
            // 进口压力（无符号16位，1kPa精度，0-1000kPa）
            uint16_t inlet_pressure = pressure_dist(gen);
            frame.data[0] = inlet_pressure & 0xFF;         // 低8位
            frame.data[1] = (inlet_pressure >> 8) & 0xFF;  // 高8位

            // 出口压力（无符号16位，1kPa精度，0-1000kPa）
            uint16_t outlet_pressure = pressure_dist(gen);
            frame.data[2] = outlet_pressure & 0xFF;         // 低8位
            frame.data[3] = (outlet_pressure >> 8) & 0xFF;  // 高8位

            // 空气温度（无符号8位，1℃精度，0-150℃）
            frame.data[4] = temp_dist(gen);

            // 5-7保留为0
            frame.data[5] = 0;
            frame.data[6] = 0;
            frame.data[7] = 0;

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback9 send failed");
            }

            // 输出发送的CAN ID和压力、温度信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t进口压力: " << std::dec << inlet_pressure << "kPa"
                         << "\t出口压力: " << outlet_pressure << "kPa"
                         << "\t空气温度: " << static_cast<int>(frame.data[4]) << "℃" << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback10Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> fault_dist(0, 255);  // 故障报警位 0-255

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback10_id_;
            frame.can_dlc = 8;
            
            // 故障报警位-1（无符号8位，0-255）
            frame.data[0] = fault_dist(gen);

            // 故障报警位-2（无符号8位，0-255）
            frame.data[1] = fault_dist(gen);

            // 故障报警位-3（无符号8位，0-255）
            frame.data[2] = fault_dist(gen);

            // 故障报警位-4（无符号8位，0-255）
            frame.data[3] = fault_dist(gen);

            // 故障报警位-5（无符号8位，0-255）
            frame.data[4] = fault_dist(gen);

            // 故障报警位-6（无符号8位，0-255）
            frame.data[5] = fault_dist(gen);

            // 故障报警位-7（无符号8位，0-255）
            frame.data[6] = fault_dist(gen);

            // 故障报警位-8（无符号8位，0-255）
            frame.data[7] = fault_dist(gen);

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback10 send failed");
            }

            // 输出发送的CAN ID和故障报警位信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t故障报警位-1: " << std::dec << static_cast<int>(frame.data[0])
                         << "\t故障报警位-2: " << static_cast<int>(frame.data[1])
                         << "\t故障报警位-3: " << static_cast<int>(frame.data[2])
                         << "\t故障报警位-4: " << static_cast<int>(frame.data[3])
                         << "\t故障报警位-5: " << static_cast<int>(frame.data[4])
                         << "\t故障报警位-6: " << static_cast<int>(frame.data[5])
                         << "\t故障报警位-7: " << static_cast<int>(frame.data[6])
                         << "\t故障报警位-8: " << static_cast<int>(frame.data[7]) << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback11Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> fault_dist(0, 255);  // 故障报警位 0-255

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback11_id_;
            frame.can_dlc = 8;
            
            // 故障报警位-9（无符号8位，0-255）
            frame.data[0] = fault_dist(gen);

            // 故障报警位-10（无符号8位，0-255）
            frame.data[1] = fault_dist(gen);

            // 故障报警位-11（无符号8位，0-255）
            frame.data[2] = fault_dist(gen);

            // 故障报警位-12（无符号8位，0-255）
            frame.data[3] = fault_dist(gen);

            // 故障报警位-13（无符号8位，0-255）
            frame.data[4] = fault_dist(gen);

            // 故障报警位-14（无符号8位，0-255）
            frame.data[5] = fault_dist(gen);

            // 故障报警位-15（无符号8位，0-255）
            frame.data[6] = fault_dist(gen);

            // 故障报警位-16（无符号8位，0-255）
            frame.data[7] = fault_dist(gen);

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback11 send failed");
            }

            // 输出发送的CAN ID和故障报警位信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t故障报警位-9: " << std::dec << static_cast<int>(frame.data[0])
                         << "\t故障报警位-10: " << static_cast<int>(frame.data[1])
                         << "\t故障报警位-11: " << static_cast<int>(frame.data[2])
                         << "\t故障报警位-12: " << static_cast<int>(frame.data[3])
                         << "\t故障报警位-13: " << static_cast<int>(frame.data[4])
                         << "\t故障报警位-14: " << static_cast<int>(frame.data[5])
                         << "\t故障报警位-15: " << static_cast<int>(frame.data[6])
                         << "\t故障报警位-16: " << static_cast<int>(frame.data[7]) << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    void autoFeedback12Loop() {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uint8_t> fault_dist(0, 255);  // 故障报警位 0-255

        while (running_) {
            struct can_frame frame;
            memset(&frame, 0, sizeof(frame));

            frame.can_id = auto_feedback12_id_;
            frame.can_dlc = 8;
            
            // 故障报警位-17（无符号8位，0-255）
            frame.data[0] = fault_dist(gen);

            // 故障报警位-18（无符号8位，0-255）
            frame.data[1] = fault_dist(gen);

            // 2-7保留为0
            frame.data[2] = 0;
            frame.data[3] = 0;
            frame.data[4] = 0;
            frame.data[5] = 0;
            frame.data[6] = 0;
            frame.data[7] = 0;

            if (write(sock_, &frame, sizeof(frame)) < 0) {
                perror("Auto feedback12 send failed");
            }

            // 输出发送的CAN ID和故障报警位信息
            uint32_t frameID = frame.can_id & CAN_EFF_MASK;
            {
                std::lock_guard<std::mutex> lock(print_mutex_);
                std::cout << "Sent CAN ID: 0x" << std::hex << frameID 
                         << "\t故障报警位-17: " << std::dec << static_cast<int>(frame.data[0])
                         << "\t故障报警位-18: " << static_cast<int>(frame.data[1]) << std::endl;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    const uint8_t thrusterNum_;
    int sock_;
    struct sockaddr_can addr_;
    struct ifreq ifr_;
    
    std::atomic<bool> running_;
    std::thread recv_thread_;
    std::thread feedback_thread1_;
    std::thread feedback_thread2_;
    std::thread feedback_thread3_;
    std::thread feedback_thread4_;
    std::thread feedback_thread5_;
    std::thread feedback_thread6_;
    std::thread feedback_thread7_;
    std::thread feedback_thread8_;
    std::thread feedback_thread9_;
    std::thread feedback_thread10_;
    std::thread feedback_thread11_;
    std::thread feedback_thread12_;
    
    
    // 共享数据
    std::mutex data_mutex_;
    std::mutex print_mutex_;  // 添加打印输出互斥锁
    int16_t current_speed_ = 0;
    uint8_t fault_flags1_ = 0;  // 停机类故障
    uint8_t fault_flags2_ = 0;  // 告警类故障
};

int main() {

    try {
        uint8_t thrusterNum = 0x12 ;
        ThrusterDriver driver(thrusterNum);
        
        std::cout << "Starting thruster " << static_cast<int>(thrusterNum) 
                  << " driver..." << std::endl;
        
        driver.start();
        
        // 主线程等待退出
        while (true) {
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }

    return 0;
}